#include "MahonyAHRS.h"
#include <math.h>

#define Beta	200.0f
#define MahonyAHRS_SampleFreq		1000.0f//采样频率(四元数更新频率)
#define MahonyAHRS_AccelMagMixKp	(2.0f * 0.5f)//(2.0f*Beta)//(2.0f * 0.5f)//加速度计以及磁力计融合权重Kp,值越大,收敛速度越快
#define MahonyAHRS_AccelMagMixKi	(2.0f * 0.0f)//(500.0f)//(Beta*Beta)//(2.0f * 0.0f)//加速度计以及磁力计融合权重Ki,值越大,收敛速度越快。
//#define Beta	0.5f
//#define MahonyAHRS_SampleFreq		1000.0f//采样频率(四元数更新频率)
//#define MahonyAHRS_AccelMagMixKp	(2.0f*Beta)//(2.0f * 0.5f)//加速度计以及磁力计融合权重Kp,值越大,收敛速度越快
//#define MahonyAHRS_AccelMagMixKi	0.0f//Beta*Beta)//(2.0f * 0.0f)//加速度计以及磁力计融合权重Ki,值越大,收敛速度越快。

float MahonyAHRS_integralFBx=0.0f,MahonyAHRS_integralFBy=0.0f,MahonyAHRS_integralFBz=0.0f;//三轴误差积分值

/*
 *函数简介:平方根倒数函数
 *参数说明:参数x
 *返回类型:返回1/√x
 *备注:不懂
 */
float invSqrt(float x)
{
	float halfx=0.5f*x;
	float y=x;
	long i=*(long*)&y;
	i=0x5f3759df-(i>>1);
	y=*(float*)&i;
	y=y*(1.5f-(halfx*y*y));
	return y;
}

/*
 *函数简介:MahonyAHRS四元数九轴更新函数
 *参数说明:四元数数组
 *参数说明:三轴加速度数据
 *参数说明:三轴角速度数据
 *参数说明:三轴磁场数据
 *返回类型:无
 *备注:在加速度与磁力补偿角速度时采用一个PI环,Kp和Ki在上方宏定义MahonyAHRS_AccelMagMixKp和MahonyAHRS_AccelMagMixKi给出
 *四元数更新公式:
 *		q0(n)=q0(n-1)+0.5t(-q1(n-1)*Gyro_x-q2(n-1)*Gyro_y-q3(n-1)*Gyro_z)
 *		q1(n)=q1(n-1)+0.5t(q0(n-1)*Gyro_x+q2(n-1)*Gyro_z-q3(n-1)*Gyro_y)
 *		q2(n)=q2(n-1)+0.5t(q0(n-1)*Gyro_y-q1(n-1)*Gyro_z+q3(n-1)*Gyro_x)
 *		q3(n)=q3(n-1)+0.5t(q0(n-1)*Gyro_z+q1(n-1)*Gyro_y-q2(n-1)*Gyro_z)
 */
void MahonyAHRS_NineAxesUpdate(float *q,float Accel_x,float Accel_y,float Accel_z,float Gyro_x,float Gyro_y,float Gyro_z,float Mag_x,float Mag_y,float Mag_z)
{
	float NormalizationCoefficient;//归一化系数
    float q0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3; //辅助参量

	if(Accel_x!=0 || Accel_y!=0 || Accel_z!=0)//传入数据有效,开始补偿角速度数据
	{
		//归一化加速度数据
		NormalizationCoefficient=invSqrt(Accel_x*Accel_x+Accel_y*Accel_y+Accel_z*Accel_z);
		Accel_x*=NormalizationCoefficient;
		Accel_y*=NormalizationCoefficient;
		Accel_z*=NormalizationCoefficient;
		
		//归一化磁场数据
		NormalizationCoefficient=invSqrt(Mag_x*Mag_x+Mag_y*Mag_y+Mag_z*Mag_z);
		Mag_x*=NormalizationCoefficient;
		Mag_y*=NormalizationCoefficient;
		Mag_z*=NormalizationCoefficient;

        q0q0=q[0]*q[0];
        q0q1=q[0]*q[1];
        q0q2=q[0]*q[2];
        q0q3=q[0]*q[3];
        q1q1=q[1]*q[1];
        q1q2=q[1]*q[2];
        q1q3=q[1]*q[3];
        q2q2=q[2]*q[2];
        q2q3=q[2]*q[3];
        q3q3=q[3]*q[3];   

        //地磁场的实际方向
		float hx,hy,bx,bz;
        hx=2.0f*(Mag_x*(0.5f-q2q2-q3q3)+Mag_y*(q1q2-q0q3)+Mag_z*(q1q3 + q0q2));
        hy=2.0f*(Mag_x*(q1q2+q0q3)+Mag_y*(0.5f-q1q1-q3q3)+Mag_z*(q2q3-q0q1));
        bx=sqrt(hx*hx+hy*hy);
        bz=2.0f*(Mag_x*(q1q3-q0q2)+Mag_y*(q2q3+q0q1)+Mag_z*(0.5f-q1q1-q2q2));

		//重力场和磁力场的计算方向
		float halfvx,halfvy,halfvz,halfwx,halfwy,halfwz;
		halfvx=q1q3-q0q2;
		halfvy=q0q1+q2q3;
		halfvz=q0q0-0.5f+q3q3;
        halfwx=bx*(0.5f-q2q2-q3q3)+bz*(q1q3-q0q2);
        halfwy=bx*(q1q2-q0q3)+bz*(q0q1+q2q3);
        halfwz=bx*(q0q2+q1q3)+bz*(0.5f-q1q1-q2q2);  
	
		//得到误差(测量方向和计算方向的叉乘)
		float halfex,halfey,halfez;
		halfex=(Accel_y*halfvz-Accel_z*halfvy)+(Mag_y*halfwz-Mag_z*halfwy);
		halfey=(Accel_z*halfvx-Accel_x*halfvz)+(Mag_z*halfwx-Mag_x*halfwz);
		halfez=(Accel_x*halfvy-Accel_y*halfvx)+(Mag_x*halfwy-Mag_y*halfwx);

		//角速度数据I补偿
		if(MahonyAHRS_AccelMagMixKi>0.0f)
		{
			MahonyAHRS_integralFBx+=MahonyAHRS_AccelMagMixKi*halfex*(1.0f/MahonyAHRS_SampleFreq);
			MahonyAHRS_integralFBy+=MahonyAHRS_AccelMagMixKi*halfey*(1.0f/MahonyAHRS_SampleFreq);
			MahonyAHRS_integralFBz+=MahonyAHRS_AccelMagMixKi*halfez*(1.0f/MahonyAHRS_SampleFreq);
			Gyro_x+=MahonyAHRS_integralFBx;
			Gyro_y+=MahonyAHRS_integralFBy;
			Gyro_z+=MahonyAHRS_integralFBz;
		}
		else
		{
			MahonyAHRS_integralFBx=0.0f;
			MahonyAHRS_integralFBy=0.0f;
			MahonyAHRS_integralFBz=0.0f;
		}

		//角速度数据P补偿
		Gyro_x+=MahonyAHRS_AccelMagMixKp*halfex;
		Gyro_y+=MahonyAHRS_AccelMagMixKp*halfey;
		Gyro_z+=MahonyAHRS_AccelMagMixKp*halfez;
	}
	
	//更新四元数
	Gyro_x*=(0.5f*(1.0f/MahonyAHRS_SampleFreq));//角速度预乘公因数0.5t
	Gyro_y*=(0.5f*(1.0f/MahonyAHRS_SampleFreq));
	Gyro_z*=(0.5f*(1.0f/MahonyAHRS_SampleFreq));
	float q0=q[0],q1=q[1],q2=q[2],q3=q[3];
	q[0]+=(-q1*Gyro_x-q2*Gyro_y-q3*Gyro_z);
	q[1]+=(q0*Gyro_x+q2*Gyro_z-q3*Gyro_y);
	q[2]+=(q0*Gyro_y-q1*Gyro_z+q3*Gyro_x);
	q[3]+=(q0*Gyro_z+q1*Gyro_y-q2*Gyro_x); 
	
	//归一化四元数
	NormalizationCoefficient=invSqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
	q[0]*=NormalizationCoefficient;
	q[1]*=NormalizationCoefficient;
	q[2]*=NormalizationCoefficient;
	q[3]*=NormalizationCoefficient;
}

/*
 *函数简介:MahonyAHRS四元数六轴更新函数
 *参数说明:四元数数组
 *参数说明:三轴加速度数据
 *参数说明:三轴角速度数据
 *返回类型:无
 *备注:在加速度补偿角速度时采用一个PI环,Kp和Ki在上方宏定义MahonyAHRS_AccelMagMixKp和MahonyAHRS_AccelMagMixKi给出
 *四元数更新公式:
 *		q0(n)=q0(n-1)+0.5t(-q1(n-1)*Gyro_x-q2(n-1)*Gyro_y-q3(n-1)*Gyro_z)
 *		q1(n)=q1(n-1)+0.5t(q0(n-1)*Gyro_x+q2(n-1)*Gyro_z-q3(n-1)*Gyro_y)
 *		q2(n)=q2(n-1)+0.5t(q0(n-1)*Gyro_y-q1(n-1)*Gyro_z+q3(n-1)*Gyro_x)
 *		q3(n)=q3(n-1)+0.5t(q0(n-1)*Gyro_z+q1(n-1)*Gyro_y-q2(n-1)*Gyro_z)
 */
void MahonyAHRS_SixAxesUpdate(float *q,float Accel_x,float Accel_y,float Accel_z,float Gyro_x,float Gyro_y,float Gyro_z)
{
	float NormalizationCoefficient;//归一化系数

	if(Accel_x!=0 || Accel_y!=0 || Accel_z!=0)//传入数据有效,开始补偿角速度数据
	{
		//归一化加速度数据
		NormalizationCoefficient=invSqrt(Accel_x*Accel_x+Accel_y*Accel_y+Accel_z*Accel_z);
		Accel_x*=NormalizationCoefficient;
		Accel_y*=NormalizationCoefficient;
		Accel_z*=NormalizationCoefficient;

		//重力场的计算方向
		float halfvx,halfvy,halfvz;
		halfvx=q[1]*q[3]-q[0]*q[2];
		halfvy=q[0]*q[1]+q[2]*q[3];
		halfvz=0.5f-q[0]*q[0]-q[1]*q[1];
	
		//得到误差(测量方向和计算方向的叉乘)
		float halfex,halfey,halfez;
		halfex=(Accel_y*halfvz-Accel_z*halfvy);
		halfey=(Accel_z*halfvx-Accel_x*halfvz);
		halfez=(Accel_x*halfvy-Accel_y*halfvx);

		//角速度数据I补偿
		if(MahonyAHRS_AccelMagMixKi>0.0f)
		{
			MahonyAHRS_integralFBx+=MahonyAHRS_AccelMagMixKi*halfex*(1.0f/MahonyAHRS_SampleFreq);
			MahonyAHRS_integralFBy+=MahonyAHRS_AccelMagMixKi*halfey*(1.0f/MahonyAHRS_SampleFreq);
			MahonyAHRS_integralFBz+=MahonyAHRS_AccelMagMixKi*halfez*(1.0f/MahonyAHRS_SampleFreq);
			Gyro_x+=MahonyAHRS_integralFBx;
			Gyro_y+=MahonyAHRS_integralFBy;
			Gyro_z+=MahonyAHRS_integralFBz;
		}
		else
		{
			MahonyAHRS_integralFBx=0.0f;
			MahonyAHRS_integralFBy=0.0f;
			MahonyAHRS_integralFBz=0.0f;
		}

		//角速度数据P补偿
		Gyro_x+=MahonyAHRS_AccelMagMixKp*halfex;
		Gyro_y+=MahonyAHRS_AccelMagMixKp*halfey;
		Gyro_z+=MahonyAHRS_AccelMagMixKp*halfez;
		if(Gyro_x<0.02f && Gyro_x>-0.02f)Gyro_x=0;
		if(Gyro_y<0.02f && Gyro_y>-0.02f)Gyro_y=0;
		if(Gyro_z<0.02f && Gyro_z>-0.02f)Gyro_z=0;
	}
	
	//更新四元数
	Gyro_x*=(0.5f*(1.0f/MahonyAHRS_SampleFreq));//角速度预乘公因数0.5t
	Gyro_y*=(0.5f*(1.0f/MahonyAHRS_SampleFreq));
	Gyro_z*=(0.5f*(1.0f/MahonyAHRS_SampleFreq));
	float q0=q[0],q1=q[1],q2=q[2],q3=q[3];
	q[0]+=(-q1*Gyro_x-q2*Gyro_y-q3*Gyro_z);
	q[1]+=(q0*Gyro_x+q2*Gyro_z-q3*Gyro_y);
	q[2]+=(q0*Gyro_y-q1*Gyro_z+q3*Gyro_x);
	q[3]+=(q0*Gyro_z+q1*Gyro_y-q2*Gyro_x); 
	
	//归一化四元数
	NormalizationCoefficient=invSqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
	q[0]*=NormalizationCoefficient;
	q[1]*=NormalizationCoefficient;
	q[2]*=NormalizationCoefficient;
	q[3]*=NormalizationCoefficient;
}

/*
 *函数简介:MahonyAHRS角度计算函数
 *参数说明:四元数数组
 *参数说明:偏航角yaw的地址
 *参数说明:俯仰角pitch的地址
 *参数说明:翻滚角roll的地址
 *返回类型:无
 *备注:无
 *欧拉角计算公式:
 *					  q0q3+q1q2
 *		yaw=arctan(---------------)
 *					q0q0+q1q1-0.5
 *
 *		pitch=arcsin(2q0q2-2q1q3)
 *
 *					   q0q1+q2q3
 *		roll=arctan(---------------)
 *					 q0q0+q3q3-0.5
 */
void MahonyAHRS_GetAngle(float *q,float *yaw,float *pitch,float *roll)
{
    *yaw=atan2f(2.0f*(q[0]*q[3]+q[1]*q[2]),2.0f*(q[0]*q[0]+q[1]*q[1])-1.0f);
    *pitch=asinf(-2.0f*(q[1]*q[3]-q[0]*q[2]));
    *roll=atan2f(2.0f*(q[0]*q[1]+q[2]*q[3]),2.0f*(q[0]*q[0]+q[3]*q[3])-1.0f);
}


/*原始代码*/
////=====================================================================================================
//// MahonyAHRS.c
////=====================================================================================================
////
//// Madgwick's implementation of Mayhony's AHRS algorithm.
//// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
////
//// Date			Author			Notes
//// 29/09/2011	SOH Madgwick    Initial release
//// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
////
////=====================================================================================================

////---------------------------------------------------------------------------------------------------
//// Header files

//#include "MahonyAHRS.h"
//#include <math.h>

////---------------------------------------------------------------------------------------------------
//// Definitions

//#define sampleFreq	1000.0f			// sample frequency in Hz
//#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
//#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain

////---------------------------------------------------------------------------------------------------
//// Variable definitions

//volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
//volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
////volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
//volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki

////---------------------------------------------------------------------------------------------------
//// Function declarations

//float invSqrt(float x);

////====================================================================================================
//// Functions

////---------------------------------------------------------------------------------------------------
//// AHRS algorithm update

//void MahonyAHRSupdate(float q[4], float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
//	float recipNorm;
//    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
//	float hx, hy, bx, bz;
//	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
//	float halfex, halfey, halfez;
//	float qa, qb, qc;

//	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
//	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
//		MahonyAHRSupdateIMU(q, gx, gy, gz, ax, ay, az);
//		return;
//	}

//	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
//	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

//		// Normalise accelerometer measurement
//		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
//		ax *= recipNorm;
//		ay *= recipNorm;
//		az *= recipNorm;     

//		// Normalise magnetometer measurement
//		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
//		mx *= recipNorm;
//		my *= recipNorm;
//		mz *= recipNorm;   

//        // Auxiliary variables to avoid repeated arithmetic
//        q0q0 = q[0] * q[0];
//        q0q1 = q[0] * q[1];
//        q0q2 = q[0] * q[2];
//        q0q3 = q[0] * q[3];
//        q1q1 = q[1] * q[1];
//        q1q2 = q[1] * q[2];
//        q1q3 = q[1] * q[3];
//        q2q2 = q[2] * q[2];
//        q2q3 = q[2] * q[3];
//        q3q3 = q[3] * q[3];   

//        // Reference direction of Earth's magnetic field
//        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
//        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
//        bx = sqrt(hx * hx + hy * hy);
//        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

//		// Estimated direction of gravity and magnetic field
//		halfvx = q1q3 - q0q2;
//		halfvy = q0q1 + q2q3;
//		halfvz = q0q0 - 0.5f + q3q3;
//        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
//        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
//        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
//	
//		// Error is sum of cross product between estimated direction and measured direction of field vectors
//		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
//		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
//		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

//		// Compute and apply integral feedback if enabled
//		if(twoKi > 0.0f) {
//			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
//			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
//			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
//			gx += integralFBx;	// apply integral feedback
//			gy += integralFBy;
//			gz += integralFBz;
//		}
//		else {
//			integralFBx = 0.0f;	// prevent integral windup
//			integralFBy = 0.0f;
//			integralFBz = 0.0f;
//		}

//		// Apply proportional feedback
//		gx += twoKp * halfex;
//		gy += twoKp * halfey;
//		gz += twoKp * halfez;
//	}
//	
//	// Integrate rate of change of quaternion
//	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
//	gy *= (0.5f * (1.0f / sampleFreq));
//	gz *= (0.5f * (1.0f / sampleFreq));
//	qa = q[0];
//	qb = q[1];
//	qc = q[2];
//	q[0] += (-qb * gx - qc * gy - q[3] * gz);
//	q[1] += (qa * gx + qc * gz - q[3] * gy);
//	q[2] += (qa * gy - qb * gz + q[3] * gx);
//	q[3] += (qa * gz + qb * gy - qc * gx); 
//	
//	// Normalise quaternion
//	recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
//	q[0] *= recipNorm;
//	q[1] *= recipNorm;
//	q[2] *= recipNorm;
//	q[3] *= recipNorm;
//}

////---------------------------------------------------------------------------------------------------
//// IMU algorithm update

//void MahonyAHRSupdateIMU(float q[4], float gx, float gy, float gz, float ax, float ay, float az) {
//	float recipNorm;
//	float halfvx, halfvy, halfvz;
//	float halfex, halfey, halfez;
//	float qa, qb, qc;

//	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
//	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

//		// Normalise accelerometer measurement
//		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
//		ax *= recipNorm;
//		ay *= recipNorm;
//		az *= recipNorm;        

//		// Estimated direction of gravity and vector perpendicular to magnetic flux
//		halfvx = q[1] * q[3] - q[0] * q[2];
//		halfvy = q[0] * q[1] + q[2] * q[3];
//		halfvz = q[0] * q[0] - 0.5f + q[3] * q[3];
//	
//		// Error is sum of cross product between estimated and measured direction of gravity
//		halfex = (ay * halfvz - az * halfvy);
//		halfey = (az * halfvx - ax * halfvz);
//		halfez = (ax * halfvy - ay * halfvx);

//		// Compute and apply integral feedback if enabled
//		if(twoKi > 0.0f) {
//			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
//			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
//			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
//			gx += integralFBx;	// apply integral feedback
//			gy += integralFBy;
//			gz += integralFBz;
//		}
//		else {
//			integralFBx = 0.0f;	// prevent integral windup
//			integralFBy = 0.0f;
//			integralFBz = 0.0f;
//		}

//		// Apply proportional feedback
//		gx += twoKp * halfex;
//		gy += twoKp * halfey;
//		gz += twoKp * halfez;
//	}
//	
//	// Integrate rate of change of quaternion
//	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
//	gy *= (0.5f * (1.0f / sampleFreq));
//	gz *= (0.5f * (1.0f / sampleFreq));
//	qa = q[0];
//	qb = q[1];
//	qc = q[2];
//	q[0] += (-qb * gx - qc * gy - q[3] * gz);
//	q[1] += (qa * gx + qc * gz - q[3] * gy);
//	q[2] += (qa * gy - qb * gz + q[3] * gx);
//	q[3] += (qa * gz + qb * gy - qc * gx); 
//	
//	// Normalise quaternion
//	recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
//	q[0] *= recipNorm;
//	q[1] *= recipNorm;
//	q[2] *= recipNorm;
//	q[3] *= recipNorm;
//}

////---------------------------------------------------------------------------------------------------
//// Fast inverse square-root
//// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

//float invSqrt(float x) {
//	float halfx = 0.5f * x;
//	float y = x;
//	long i = *(long*)&y;
//	i = 0x5f3759df - (i>>1);
//	y = *(float*)&i;
//	y = y * (1.5f - (halfx * y * y));
//	return y;
//}

////====================================================================================================
//// END OF CODE
////====================================================================================================
